#include <fstream>

#include "ubt_mqtt/task_manager.h"
#include "ubt_mqtt/node.h"

namespace ubt
{

CTaskManager::CTaskManager(std::shared_ptr<CMapManager> ptr)
{
    _RequsetId = "";
    _mapManagerStr = ptr;

    ubt_yaml::getValue(ConfigDataInfo::Instance().getTaskFilePath(), "task", _taskInfo);
}

CTaskManager::~CTaskManager()
{

}

bool CTaskManager::UpdateTaskInfo(const Json::Value& taskJson)
{
    try
    {
        _taskInfo.id   = taskJson["task_id"].asString();
        _taskInfo.name = taskJson["task_name"].asString();
        _taskInfo.code = taskJson["task_code"].asString();

        {
            YAML::Node map_node;
            map_node["task"].push_back(_taskInfo.id);
            map_node["task"].push_back(_taskInfo.name);
            map_node["task"].push_back(_taskInfo.code);

            std::ofstream fout(ConfigDataInfo::Instance().getTaskFilePath().c_str());
            fout << map_node;
            fout.close();
        }
    }
    catch(const std::exception& e)
    {
        RCLCPP_ERROR_STREAM(node->get_logger(),"prase task info error: " << e.what() << '\n');
        return false;
    }

    return true;
}

bool CTaskManager::SaveRealPath(Json::Value& paths)
{
    YAML::Node yml_node;

    for (int i = 0; i < paths.size(); i++)
    {
        YAML::Node pathNode;

        pathNode["path"] = i;
        pathNode["type"] = paths[i]["type"].asString();

        Json::Value points = paths[i]["points"];
        for (int j = 0; j < points.size(); j++)
        {
            location_pose_t pose;
            location_pose_t destpose;

            pose.x = points[j]["x"].asFloat();
            pose.y = points[j]["y"].asFloat();
            pose.theta = points[j]["t"].asFloat();
            if (_mapManagerStr)
            {
                _mapManagerStr->MapToWorld(pose, destpose);
            }

            std::string key = "point" + std::to_string(j);
            pathNode[key].push_back(destpose.x);
            pathNode[key].push_back(destpose.y);
            pathNode[key].push_back(destpose.theta);

            paths[i]["points"][j]["x"] = destpose.x;
            paths[i]["points"][j]["y"] = destpose.y;
            paths[i]["points"][j]["t"] = destpose.theta;
        }

        pathNode["limit_v"] = paths[i]["limit"]["x"].asFloat();
        pathNode["limit_w"] = paths[i]["limit"]["w"].asFloat();

        yml_node.push_back(pathNode);
    }

    RCLCPP_INFO_STREAM(node->get_logger(),"task yaml: " << yml_node);
    std::ofstream fout(ConfigDataInfo::Instance().getFixedPathFilePath().c_str());
    fout << yml_node;
    fout.close();
    return true;
}

bool CTaskManager::HandleRealTaskData(const Json::Value& root, const std::function<void(const std::string &state)> &notify_callback)
{
    Json::Value fixedpath_task;
    Json::Value paths = root["data"]["path"];

    _RequsetId = root["request_id"].asString();

    SaveRealPath(paths);

    fixedpath_task["now_task_id"] = root["data"]["now_task_id"];
    fixedpath_task["wait_time"]   = root["data"]["wait_time"];
    fixedpath_task["actions"]     = root["data"]["actions"];
    fixedpath_task["path"]        = paths;
    fixedpath_task["props"]       = root["data"]["props"];

    if (notify_callback)
    {
        Json::StreamWriterBuilder builder;
        builder["indentation"] = ""; // 保证输出的字符串是无格式的
        notify_callback(Json::writeString(builder, fixedpath_task));
    }

    return true;
}

} // namespace ubt


